%% 卡尔曼滤波子函数
function [State, P] = KF(P, G, H, Q, R, prev_state, Sensor_Meas)
%Track_KF 此处显示有关此函数的摘要
% State 最优估计状态
% P 估计均方误差矩阵
% A 系统矩阵
% H 观测矩阵
% Q 过程噪音的协方差矩阵
% R 测量噪音的协方差矩阵
% prev_state 前一时刻的状态
% Sensor_Meas 当前时刻的测量向量
N = size(P, 1); % 计算状态变量的个数
%% Predict
pred_state = G * prev_state;                                    % 预测状态
pred_P = G * P * G' + Q;                                        % 预测误差的协方差矩阵

%% Update
    S = inv(H * pred_P * H' + R);                               % 矩阵求逆
    K = pred_P * H' * S;                                        % 计算卡尔曼增益
    State = pred_state + K * (Sensor_Meas - H * pred_state);    % 状态更新方程
    P = (eye(N) - K * H) * pred_P;                              % 估计均方误差矩阵的更新，用于下一次迭代
end
